

#ifndef MATRIX3_H
#define MATRIX3_H

#include <Vec4.h>

// needed for memcpy - but will get rid of those memcpy btw
#include <iostream>

#define swap_matrix3(a,b) { float c = (a); (a) = (b); (b) = c; }

// TO DO: get rid of all branches, unroll completely


	class Matrix3
	{
		
		/* elements indexed as [row][col]  == row + (4 * col), i.e. 
		 
		 00 01 02   ->  0  3  6 
		 10 11 12   ->  1  4  7 
		 20 21 22   ->  2  5  8
		 
		 */
		
	public:
		
		//initialization
		Matrix3()
		{
			setIdentity();
		}
		
		Matrix3(const float* m)
		{
			std::memcpy( m_matrix, m, 9*sizeof(float) );
		}
		
		Matrix3(const Matrix3& other)
		{
			std::memcpy( m_matrix, other.m_matrix, 9*sizeof(float) );
		}
		
		inline void setColumn(int col, Vec4 colVec)
		{
			int col3 = 3*col;
			m_matrix[col3]   = colVec[0];
			m_matrix[col3+1] = colVec[1];
			m_matrix[col3+2] = colVec[2];
		}
		
		inline Vec4 getColumn(int col) const
		{
			int col3 = 3*col;
			Vec4 colVec(m_matrix[col3], m_matrix[col3+1], m_matrix[col3+2]);
			return colVec;
		}
		
		inline void setIdentity()
		{
			std::memset( m_matrix, 0, 9*sizeof(float) );
			m_matrix[0] = 1.0f;
			m_matrix[4] = 1.0f;
			m_matrix[8] = 1.0f;
		}
		
		inline void setZero()
		{
			std::memset( m_matrix, 0, 9*sizeof(float) );
		}
		
		// assignment
		Matrix3& operator=(const Matrix3& rhs)
		{
			std::memcpy( m_matrix, rhs.m_matrix, 9*sizeof(float) );
			return *this;
		}
		
		// invert 3x3 matrix (return false if singular)
		bool invert();
		
		// invert a symmetric, positive-definite 3x3 matrix
		void invertSymmetricPositiveDefinite();

		// Trace
		float trace();

		// Sets the translation column to the given vector
		void setTranslation(const Vec4& translation);
		Vec4 getTranslation() const;
			
		// ALGEBRA
		
		//element access
		float& operator[](const int index);
		const float& operator[](const int index) const;
		
		inline float& operator() (int r, int c);
		inline const float& operator() (int r, int c) const;
		
		// return transpose
		Matrix3 getTranspose() const;
		
		// transpose in-place
		void transpose();
		
		// addition and subtraction
		friend Matrix3& operator+=( Matrix3& lhs, const Matrix3& rhs );
		friend Matrix3 operator+( const Matrix3& lhs, const Matrix3& rhs );
		
		friend Matrix3& operator-=( Matrix3& lhs, const Matrix3& rhs );
		friend Matrix3 operator-( const Matrix3& lhs, const Matrix3& rhs );
		
		// multiplication (matrix*matrix and float*matrix)
		friend Matrix3& operator*=( Matrix3& lhs, const Matrix3& rhs );
		friend Matrix3 operator*( const Matrix3& lhs, const Matrix3& rhs );
		
		friend Matrix3& operator*=( Matrix3& lhs, float rhs );
		friend Matrix3 operator*( float lhs, const Matrix3& rhs );
		
		// right multiply a Matrix3 by a Vec4 (sets w-component of output Vec4 to 0) 
		friend Vec4 operator*( const Matrix3& lhs, const Vec4& rhs );
		
		// Diagonalization
		bool diagonalizeSymmetric(Matrix3& eigenVec, Vec4& eigenVal, int maxIter, float epsilon) const;
		
		// Do polar decomposition M = RS
		void polarDecompose(Matrix3& symmetricOut, Matrix3& rotationOut, int maxIter, float epsilon) const;
		
	public:	
		
		float m_matrix[9];
		
	};
	
	inline float& Matrix3::operator[](const int index)
	{
		return m_matrix[index];
	}
	
	inline const float& Matrix3::operator[](const int index) const
	{
		return m_matrix[index];
	}
	
	inline float& Matrix3::operator() (int row, int col)
	{
		return m_matrix[row + (3*col)];
	}
	
	inline const float& Matrix3::operator() (int row, int col) const
	{
		return m_matrix[row + (3*col)];
	}
	
	inline Matrix3 Matrix3::getTranspose() const
	{
		Matrix3 Tr = *this;
		swap_matrix3(Tr.m_matrix[3], Tr.m_matrix[1]);
		swap_matrix3(Tr.m_matrix[6], Tr.m_matrix[2]);
		swap_matrix3(Tr.m_matrix[7], Tr.m_matrix[5]);
		return Tr;
	}
	
	inline void Matrix3::transpose()
	{
		swap_matrix3(m_matrix[3], m_matrix[1]);
		swap_matrix3(m_matrix[6], m_matrix[2]);
		swap_matrix3(m_matrix[7], m_matrix[5]);
	}
	
	inline Matrix3& operator+=( Matrix3& lhs, const Matrix3& rhs )
	{
		for (int i=0; i<9; ++i)
		{
			lhs.m_matrix[i] += rhs.m_matrix[i];
		}
		return lhs;
	}
	
	inline Matrix3& operator-=( Matrix3& lhs, const Matrix3& rhs )
	{
		for (int i=0; i<9; ++i)
		{
			lhs.m_matrix[i] -= rhs.m_matrix[i];
		}
		return lhs;
	}
	
	inline Matrix3 operator+( const Matrix3& lhs, const Matrix3& rhs )
	{
		Matrix3 ret(lhs);
		ret += rhs;
		return ret;
	}
	
	inline Matrix3 operator-( const Matrix3& lhs, const Matrix3& rhs )
	{
		Matrix3 ret(lhs);
		ret -= rhs;
		return ret;
	}
	
	inline Matrix3& operator*=( Matrix3& lhs, const Matrix3& rhs )
	{
		// TO DO: write a sensible version of this without branches
		Matrix3 mul;
		for (int i=0; i<3; ++i)
		for (int j=0; j<3; ++j)
		{
			int j3 = 3*j;
			int ij3 = i + j3;
			mul.m_matrix[ij3] = 0.0f;
			for (int k=0; k<3; ++k)
			{
				mul.m_matrix[ij3] += lhs.m_matrix[i+3*k] * rhs.m_matrix[k+j3]; 
			}
		}
		
		lhs = mul;
		return lhs;
	}
	
	inline Matrix3 operator*( const Matrix3& lhs, const Matrix3& rhs )
	{
		Matrix3 ret(lhs);
		ret *= rhs;
		return ret;
	}
	

	inline Matrix3& operator*=( Matrix3& lhs, float rhs )
	{
		Matrix3 mul;
		for (int i=0; i<3; ++i)
		for (int j=0; j<3; ++j)
		{
			int j3 = 3*j;
			int ij3 = i + j3;
			mul.m_matrix[ij3] = rhs * lhs.m_matrix[ij3];
		}
		lhs = mul;
		return lhs;
	}
	
	inline Matrix3 operator*( float lhs, const Matrix3& rhs )
	{
		Matrix3 ret(rhs);
		ret *= lhs;
		return ret;
	}
	
	inline Vec4 operator*( const Matrix3& lhs, const Vec4& rhs )
	{
		float x, y, z;
		x = y = z = 0.0f;
		
		for (int j=0; j<3; ++j)
		{			
			int i = 3*j;
			float v = rhs[j];
			x += lhs.m_matrix[i++] * v;
			y += lhs.m_matrix[i++] * v;
			z += lhs.m_matrix[i  ] * v;
		}
		
		Vec4 ret(x, y, z, 0.0f);
		return ret;		
	}
	
	
	

#endif //MATRIX3_H

